/*! @file MiniCheetah.h
 *  @brief Utility function to build a Mini Cheetah Quadruped object
 *
 * This file is based on MiniCheetahFullRotorModel_mex.m and builds a model
 * of the Mini Cheetah robot.  The inertia parameters of all bodies are
 * determined from CAD.
 *
 */

#ifndef PROJECT_ZOUWU_H
#define PROJECT_ZOUWU_H

#include "common/Dynamics/FloatingBaseModel.h"
#include "common/Dynamics/Quadruped.h"

/*!
 * Generate a Quadruped model of AlpghaDog
 */
template <typename T>
Quadruped<T> buildZouwu() {
    Quadruped<T> dog;
    dog._shortLinkLength = 0.1489;
    dog._longLinkLength = 0.1634;
    dog._ExtendLinkLength = 0.06;

    dog._bodyMass = 3.133;
    dog._bodyLength = 0.296;
    dog._bodyWidth = 0.45;
    dog._bodyHeight = 0.164;

    dog._abadLinkLength = 0.15;
    dog._hipLinkLength =  dog._shortLinkLength;
    dog._kneeLinkLength =  dog._shortLinkLength;

    dog._motorTauMax = 3.f;
    dog._jointDamping = .01;
    dog._jointDryFriction = .2;

    // unaccurate data
    dog._abadGearRatio = 6;
    dog._hipGearRatio = 6;
    dog._kneeGearRatio = 9.33;
    dog._kneeLinkY_offset = 0;
    dog._maxLegLength = 0.36;

    dog._motorTauMax = 3.f;
    dog._batteryV = 24;
    dog._motorKT = .05; 
    dog._motorR = 0.173;
    dog._jointDamping = .01;
    dog._jointDryFriction = .2;

    // rotor inertia if the rotor is oriented so it spins around the z-axis
    Mat3<T> rotorRotationalInertiaZ;
    rotorRotationalInertiaZ << 33, 0, 0, 0, 33, 0, 0, 0, 63;
    rotorRotationalInertiaZ = 1e-6 * rotorRotationalInertiaZ;

    Mat3<T> RY = coordinateRotation<T>(CoordinateAxis::Y, M_PI / 2);
    Mat3<T> RX = coordinateRotation<T>(CoordinateAxis::X, M_PI / 2);
    Mat3<T> rotorRotationalInertiaX = RY * rotorRotationalInertiaZ * RY.transpose();
    Mat3<T> rotorRotationalInertiaY = RX * rotorRotationalInertiaZ * RX.transpose();

    // spatial inertias
    Mat3<T> abadRotationalInertia;
    abadRotationalInertia << 1151, 0, 0, 0, 758, 444, 0, 444, 776;
    abadRotationalInertia = abadRotationalInertia * 1e-6;
    Vec3<T> abadCOM(0, -0.046, -0.061);    // LEFT
    SpatialInertia<T> abadInertia(0.264, abadCOM, abadRotationalInertia);

    Mat3<T> hipRotationalInertia;
    hipRotationalInertia << 202, 1, 4, 1, 179, 56, 4, 56, 31;
    hipRotationalInertia = hipRotationalInertia * 1e-6;
    Vec3<T> hipCOM(0.019, -0.113, -0.158);
    SpatialInertia<T> hipInertia(0.076, hipCOM, hipRotationalInertia);

    Mat3<T> kneeRotationalInertia, kneeRotationalInertiaRotated;
    kneeRotationalInertiaRotated << 202, -0.1, 4, -0.1, 191, -34, 4, -34, 19;
    kneeRotationalInertiaRotated = kneeRotationalInertiaRotated * 1e-6;
    kneeRotationalInertia = RY * kneeRotationalInertiaRotated * RY.transpose();
    Vec3<T> kneeCOM(-0.018, -0.107, -0.066);
    SpatialInertia<T> kneeInertia(0.076, kneeCOM, kneeRotationalInertia);

    Vec3<T> rotorCOM(0, 0, 0);
    SpatialInertia<T> rotorInertiaX(0.055, rotorCOM, rotorRotationalInertiaX);
    SpatialInertia<T> rotorInertiaY(0.055, rotorCOM, rotorRotationalInertiaY);

    Mat3<T> bodyRotationalInertia;
    bodyRotationalInertia << 45323, 0, 0, 0, 78150, 0, 0, 0, 52894;
    bodyRotationalInertia = bodyRotationalInertia * 1e-6;
    Vec3<T> bodyCOM(0, 0, 0);
    SpatialInertia<T> bodyInertia(dog._bodyMass, bodyCOM, bodyRotationalInertia);

    dog._abadInertia = abadInertia;
    dog._hipInertia = hipInertia;
    dog._kneeInertia = kneeInertia;
    dog._abadRotorInertia = rotorInertiaX;
    dog._hipRotorInertia = rotorInertiaY;
    dog._kneeRotorInertia = rotorInertiaY;
    dog._bodyInertia = bodyInertia;

    // locations
    dog._abadRotorLocation = Vec3<T>(0.148, 0.145, 0.084); // 0.156, 0.16, 0.0753
    dog._abadLocation = dog._abadRotorLocation;
    dog._hipLocation = dog._abadLocation + Vec3<T>(0.113, 0, -0.098); //  0.0883, 0, -0.116
    dog._hipRotorLocation = dog._hipLocation;
    dog._kneeLocation = dog._hipLocation;
    dog._kneeRotorLocation = dog._kneeLocation;

    return dog;
}

#endif    // PROJECT_ZOUWU_H
